/*!
 * MindPlus
 * pico
 *
 */
#define LEFT_MOTOR_PWM 10
#define LEFT_MOTOR_DIR1 11
#define LEFT_MOTOR_DIR2 12
#define RIGHT_MOTOR_PWM 15
#define RIGHT_MOTOR_DIR1 14
#define RIGHT_MOTOR_DIR2 13
#define BUZZER_PIN 18
#define SPEED_STEP 35
#define MIN_SPEED 80
#define MAX_SPEED 255
#define NUM_SPEED_LEVELS 6
#define SPIN_TURN_SPEED 80
#define DIAGONAL_SLOW_FACTOR 0.4
String comdata = "";
volatile float ywt_variable_speed_n = 150.0;
void beep(int frequency, int duration) {
  tone(BUZZER_PIN, frequency, duration);
  delay(duration + 50);
  noTone(BUZZER_PIN);
}
void indicateSpeedChange(bool isAccelerate) {
  if (isAccelerate) {
    beep(1200, 100);
    delay(50);
    beep(1200, 100);
  } else {
    beep(300, 200);
    delay(50);
    beep(300, 200);
  }
}
void setLeftMotor(int speed, int dir1, int dir2) {
    analogWrite(LEFT_MOTOR_PWM, speed);
    digitalWrite(LEFT_MOTOR_DIR1, dir1);
    digitalWrite(LEFT_MOTOR_DIR2, dir2);
    Serial.print("L_MOTOR: S=");
    Serial.print(speed);
    Serial.print(",D1=");
    Serial.print(dir1);
    Serial.print(",D2=");
    Serial.println(dir2);
}
void setRightMotor(int speed, int dir1, int dir2) {
    analogWrite(RIGHT_MOTOR_PWM, speed);
    digitalWrite(RIGHT_MOTOR_DIR1, dir1);
    digitalWrite(RIGHT_MOTOR_DIR2, dir2);
    Serial.print("R_MOTOR: S=");
    Serial.print(speed);
    Serial.print(",D1=");
    Serial.print(dir1);
    Serial.print(",D2=");
    Serial.println(dir2);
}
void stopMotors() {
    setLeftMotor(0, 0, 0);
    setRightMotor(0, 0, 0);
    Serial.println("MOTORS_STOPPED");
}
void moveForward() {
    Serial.println("CMD: Forward");
    int speed = (int)ywt_variable_speed_n;
    setLeftMotor(speed, 1, 0);
    setRightMotor(speed, 1, 0);
}
void moveBackward() {
    Serial.println("CMD: Backward");
    int speed = (int)ywt_variable_speed_n;
    setLeftMotor(speed, 0, 1);
    setRightMotor(speed, 0, 1);
}
void fixedTurnLeft() {
    Serial.println("CMD: Fixed Left (Spin)");
    int speed = SPIN_TURN_SPEED;
    setLeftMotor(speed, 0, 1);
    setRightMotor(speed, 1, 0);
}
void fixedTurnRight() {
    Serial.println("CMD: Fixed Right (Spin)");
    int speed = SPIN_TURN_SPEED;
    setLeftMotor(speed, 1, 0);
    setRightMotor(speed, 0, 1);
}
void moveForwardLeft() {
    Serial.println("CMD: Forward Left");
    int fastSpeed = (int)ywt_variable_speed_n;
    int slowSpeed = (int)(fastSpeed * DIAGONAL_SLOW_FACTOR);
    slowSpeed = max(MIN_SPEED, slowSpeed);
    setLeftMotor(slowSpeed, 1, 0);
    setRightMotor(fastSpeed, 1, 0);
}
void moveForwardRight() {
    Serial.println("CMD: Forward Right");
    int fastSpeed = (int)ywt_variable_speed_n;
    int slowSpeed = (int)(fastSpeed * DIAGONAL_SLOW_FACTOR);
    slowSpeed = max(MIN_SPEED, slowSpeed);
    setLeftMotor(fastSpeed, 1, 0);
    setRightMotor(slowSpeed, 1, 0);
}
void moveBackwardLeft() {
    Serial.println("CMD: Backward Left");
    int fastSpeed = (int)ywt_variable_speed_n;
    int slowSpeed = (int)(fastSpeed * DIAGONAL_SLOW_FACTOR);
    slowSpeed = max(MIN_SPEED, slowSpeed);
    setLeftMotor(slowSpeed, 0, 1);
    setRightMotor(fastSpeed, 0, 1);
}
void moveBackwardRight() {
    Serial.println("CMD: Backward Right");
    int fastSpeed = (int)ywt_variable_speed_n;
    int slowSpeed = (int)(fastSpeed * DIAGONAL_SLOW_FACTOR);
    slowSpeed = max(MIN_SPEED, slowSpeed);
    setLeftMotor(fastSpeed, 0, 1);
    setRightMotor(slowSpeed, 0, 1);
}



// 主程序开始
void setup() {
	Serial.begin(9600);
  delay(2000);
  Serial.println("--- Robot Car Control Started ---");
  Serial.print("Initial Speed Variable: ");
  Serial.println(ywt_variable_speed_n);
  Serial.print("Speed Step: ");
  Serial.println(SPEED_STEP);
  Serial.print("Min Speed: ");
  Serial.println(MIN_SPEED);
  Serial.print("Max Speed: ");
  Serial.println(MAX_SPEED);
  Serial.print("Number of Speed Levels: ");
  Serial.println(NUM_SPEED_LEVELS);
  Serial.print("Fixed Spin Turn Speed: ");
  Serial.println(SPIN_TURN_SPEED);
  Serial.print("Diagonal Slow Factor: ");
  Serial.println(DIAGONAL_SLOW_FACTOR);
  Serial.println("Debug output via USB Serial (9600 baud)");
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR1, OUTPUT);
  pinMode(LEFT_MOTOR_DIR2, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR1, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR2, OUTPUT);
  Serial.println("Motor pins initialized.");
  pinMode(BUZZER_PIN, OUTPUT);
  Serial.println("Buzzer pin initialized.");
  Serial2.begin(9600);
  Serial.println("Bluetooth Serial2 started (115200 baud)");
  stopMotors();
  Serial.println("--- Setup Complete ---");
}
void loop() {
	while (Serial2.available() > 0) {
    char receivedChar = Serial2.read();
    comdata += receivedChar;
    delay(2);
  }
  if (comdata.length() > 0) {
    Serial.print("Received Command: '");
    Serial.print(comdata);
    Serial.println("'");
    comdata.trim();
    if (comdata == "BLE_Forward") {
      moveForward();
    }
    else if (comdata == "BLE_Backward") {
      moveBackward();
    }
    else if (comdata == "BLE_C") {
      fixedTurnLeft();
    }
    else if (comdata == "BLE_D") {
      fixedTurnRight();
    }
    else if (comdata == "BLE_Forward_Left") {
      moveForwardLeft();
    }
    else if (comdata == "BLE_Forward_Right") {
      moveForwardRight();
    }
    else if (comdata == "BLE_Backward_Left") {
      moveBackwardLeft();
    }
    else if (comdata == "BLE_Backward_Right") {
      moveBackwardRight();
    }
    else if (comdata == "BLE_Stop") {
      stopMotors();
    }
    else if ((comdata == "BLE_Accelerate") && (ywt_variable_speed_n <= (MAX_SPEED - SPEED_STEP))) {
      Serial.print("CMD: Accelerate. Old Speed Var: ");
      Serial.println(ywt_variable_speed_n);
      ywt_variable_speed_n += SPEED_STEP;
      Serial.print("CMD: Accelerate. New Speed Var: ");
      Serial.println(ywt_variable_speed_n);
      indicateSpeedChange(true);
    }
    else if ((comdata == "BLE_Moderate") && (ywt_variable_speed_n >= (MIN_SPEED + SPEED_STEP))) {
      Serial.print("CMD: Moderate. Old Speed Var: ");
      Serial.println(ywt_variable_speed_n);
      ywt_variable_speed_n -= SPEED_STEP;
      Serial.print("CMD: Deccelerate. New Speed Var: ");
      Serial.println(ywt_variable_speed_n);
      indicateSpeedChange(false);
    }
    else {
      Serial.println("Unknown command");
      Serial.print(comdata);
      Serial.println("'");
    }
    comdata = "";
  }
  delay(10);
}
